#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include "joy/Joy.h"
#include <sstream>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;
  ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
  ros::Publisher joy_values = n.advertise<
  ros::Rate loop_rate(5);
  int count = 0;
//  p2os::MotorState state;
//  state.state = 1;
while (ros::ok())
  {
//    motorstate_pub.publish(state);
    geometry_msgs::Twist msg;
 //   p2os::MotorState state;
  //  state.state = 1;

    msg.linear.x = 0.2;
    msg.angular.z = 0;
    vel_pub.publish(msg);
    ROS_INFO("I published!");
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }
}
